#ifndef __BOID_H
#define __BOID_H



namespace math 
{
	namespace pbm
	{
		// Global System Configuration
		struct object_tag {}; 
		template <typename vector_type, typename object_tag> 
			struct object_state
			{
				typedef typename vector_type::value_type scalar_type; 
				vector_type velocity, position, direction; 
			} ;
		template <typename variable_type, typename object_tag> struct object_policy {};

		struct boid_tag : public object_tag {} ;
		struct obstacle_tag : public object_tag {} ;

	
		template <typename vector_type>
			struct boid_state : public object_state <vector_type, boid_tag> 
		{
			// Add some new state for boid. 
		} ;



		template <typename variable_>
			struct object_policy <variable_, boid_tag>
		{
			typedef variable_ variable_type ; 
			typedef typename variable_type::value_type value_type; 
		
			inline static variable_type gravity (const value_type & m)  {return variable_type (0.0, 0.0, -1.0) * value_type(g) * value_type(m) ;}
			inline static variable_type air_force  (const variable_type & velocity)  {return - value_type( d) * velocity ;}
			inline static variable_type wind_force (const variable_type & wind_dir)  {return  value_type(d) * wind_dir ;}
			inline static value_type vertical_bounce_velocity (const value_type & vn)  {return - value_type(epsi) * vn ;}
			inline static value_type horizontal_friction_velocity (const value_type & vt)  {return (1-miu) * vt ;}
			inline static variable_type acceleration (const variable_type & force)  {return force / value_type(mass); }
			inline static void decompose_velocity (const variable_type & velocity, const variable_type & normal, variable_type & vn, variable_type & vt)
			{
				vn =  ( velocity * normal ) * normal ;
				vt = velocity - vn ; 
			}
			static variable_type change_collide_velocity (const variable_type & velocity, const variable_type & normal)
			{
				variable_type vn, vt ;
				decompose_velocity (velocity, normal, vn, vt);
				vn = - value_type(epsi) * vn ;
				vt = (1- miu) * vt; 
				return vn + vt; 
			}

			/*
			inline static variable_type general_field_acc (const variable_type & position)
			{
				//variable_type acc ;

				variable_type acc , acc1 , acc2;
				acc1 = variable_type(- position.y, position.x, 0);
				//acc *= 5.0; 
				if ( (acc1).module2() > 1e-5)
					acc1.normalize();
				acc2 = variable_type (-position.x, -position.y, 0);
				if ( (acc2).module2() > 1e-5)
					acc2.normalize();				
				return acc1 + acc2* value_type(0.2f) ;

			}
			*/
		public :
			static constexpr value_type g = 9.8 ; // gravity coefficient ;
			static constexpr value_type d = 6.0 ; // d is air friction coefficient ; 
//   		static constexpr value_type time_step = 0.01 ; // h is the timestep ;
			static constexpr value_type epsi = 0.90 ; // bouncing coefficient; 0 \leq epsi \leq 1; 
			static constexpr value_type miu  = 0.05; // miu is horizontal friction coefficient; 
			static constexpr value_type mass = 10; // the mass of the object; 

		};

		template <typename object_tag> 
			struct object_base 
			{
				typedef float value_type; 
				typedef math::vector<value_type, 3> vector_type ;
				typedef vector_type point_type ;
				typedef vector_type color_type ;
				typedef boid_state <math::vector <value_type, 3> >  state_type; 
				typedef object_policy <math::vector <value_type, 3> ,  boid_tag> policy_type; 

				typedef math::HierarchyMesh<math::mesh_traits<value_type, 3> >   mesh_type ;
				typedef math::mesh_traits <value_type, 3>::FaceType              face_type ;
				typedef math::Polygon<value_type, 3>                         polygon_type ;
				typedef math::Sphere<value_type, 3>                          sphere_type ;
				typedef math::Segment<value_type, 3>                         segment_type ;
				typedef math::Ray<value_type, 3>                             ray_type ;

				void initialize (const state_type & s, value_type ts, value_type ls, color_type & color) 
				{ 
					this->old_state = s;
					this->m_state = s ;
					this->new_state = s ;
					time_step = ts ;
					m_lifespan = ls ;
					m_color = color ;
					m_color_decrease = m_color * value_type(0.5)/ (m_lifespan) ;
				}
				
				void set_pray_position(const vector_type & p){this->m_pray_position = p; }
				vector_type get_pray_acc ()
				{
					value_type k_pray = 2.0f; 
					vector_type acc = (this->m_pray_position - this->get_state().position).normalize();
					acc *= value_type(k_pray);
					return acc; 
				}
				vector_type get_acceleration(const vector_type & )
				{
					vector_type force (0.0, 0.0, 0.0); 
//					force += policy_type::gravity(value_type (policy_type::mass));
					force += policy_type::air_force (this->m_state.velocity) ;
//					force += policy_type::wind_force (vector_type (2.0, 0.0, 0.0));
//					force += 

					vector_type obj_acc_sum (0, 0, 0);
					for (unsigned i= 0; i < mp_boundary->size(); ++i)
					{
						
						vector_type obj_acc (0, 0, 0) ;
						
						if (compute_avoid_object_acc(obj_acc, (*mp_boundary)[i]))
						{
							obj_acc_sum +=obj_acc ;
						}
					}
					return force / value_type (policy_type::mass)  + m_avoid_acc + m_center_acc + m_velocity_acc  + obj_acc_sum +get_pray_acc ();
					/*
					if (false == compute_avoid_object_acc(obj_acc))
						return force / value_type (policy_type::mass)  + m_avoid_acc + m_center_acc + m_velocity_acc  + get_pray_acc () ;
					else 
						return force / value_type (policy_type::mass)  + m_avoid_acc + m_center_acc + m_velocity_acc  + obj_acc ;
					*/
				}
				vector_type get_velocity (const vector_type & ) 
				{
					return vector_type(2.0, 2.0, 2.0) ;
				}
				state_type moving (value_type remain_time ) 
				{
					// collect all force to check the 
					state_type state  = this->m_state;
					//math::integration <vector_type, euler_intergration> obj; 
					value_type h = this->time_step; 

					for (unsigned i = 0; i < 0.01/ h; ++i)
					{
						vector_type old_velocity = state.velocity ;
						//state.velocity = obj (old_velocity, h, boost::bind (& object_base::get_acceleration, this, _1)) ;
						//state.position = obj (state.position, h, boost::bind (& system<particle_system>::get_velocity, this, _1)) ;
						state.velocity = old_velocity + get_acceleration (vector_type ()) * h ;
						state.position += old_velocity * h; 
					}
					return state; 
				}

				void face_to_polygon (face_type * face, polygon_type & pol)
				{
					std::vector<point_type *> v_set; 
					face->Face2VertexCoord(v_set) ;
					for (unsigned i = 0; i < v_set.size(); ++i) pol.append_vertex ( * v_set[i]);
				}
				state_type collide (const state_type & old_state, const state_type & new_state, value_type & remain_time) 
				{
					/*
					face_type * collide_face = mp_boundary->faces[v_face_id[0]] ;
					vector_type face_normal = collide_face->get_normal (); 
					face_normal.normalize(); 
					polygon_type pol ; 
					face_to_polygon (collide_face, pol); 
					value_type old_dis = std::abs(distance(old_state.position, pol));
					value_type new_dis = std::abs(distance(new_state.position, pol));
					value_type f = old_dis / (old_dis + new_dis);

					state_type state ;
					vector_type collide_velocity = old_state.velocity + f * time_step * get_acceleration(old_state.velocity); 
					state.velocity = policy_type::change_collide_velocity (collide_velocity, face_normal );
					state.position = old_state.position + f * time_step * old_state.velocity; 
					remain_time = time_step * (1-f); 
					return state; 
					*/
				}
				bool is_collide (const state_type & old_state, const state_type & new_state) 
				{
					/*
					v_face_id.clear () ;
					segment_type  seg (new_state.position, old_state.position);
					return mp_boundary->intersect (seg, v_face_id); 
					*/
					return false; 
				}

				bool compute_avoid_object_acc (vector_type & acc, mesh_type * obj )
				{

					//accelerate the speed. 
				    point_type & testp =  obj->vertices[0]->m_coord ;
					if (distance2 (testp, m_state.position) > 10.0) return false; 

					v_face_id.clear () ;
					point_type p = m_state.position + (m_pray_position - m_state.position) * 100000.0f ;

					const float kp = 50.0f; 
					vector_type pray_dir = m_pray_position - m_state.position;
					if (pray_dir * m_state.velocity < 0 ) return false; 

					p = m_state.position + (m_state.velocity) * 100000.0f ;
					segment_type  seg (m_state.position, p);

					if (true == obj->segment_pick (seg, v_face_id))
					{
						face_type *  face = obj->faces[v_face_id[0]] ;
						vector_type normal = face->get_normal (); 
						normal.normalize();
						point_type & p2 = face->he->from_v->m_coord ;
						point_type & p1 = m_state.position ;
						value_type dis = distance2(p1, p2);
						//if (std::abs(dis) <= 1e-3)
						//{
						//	vector_type v = p1 - p2 ;
						//	v.normalize();
						//	return 10.0f * v; 
								
						//}
						pray_dir.normalize(); 
						//acc = pray_dir + normal * (pray_dir * normal)  ;						
						acc = normal ;
						if (dis > 10.0) return false ;
						if (dis < 3.0)
						{
							acc = normal * float (kp);
							return true; 
						}
						acc = normal / dis * kp * 3.0f ;
						return true; 
						
						
					}
					return false; 
				}

				void update () 
				{

					static unsigned count = 0; 
					value_type remain_time = time_step; 

					while (remain_time > 0 && m_state.velocity.module2() > 1e-5 )
					{
						new_state = moving(remain_time); 
						if (is_collide (this->m_state, new_state))
						{
							this->old_state = m_state ;
							this->m_state = collide (this->m_state, new_state, remain_time) ;

						}
						else
						{
							this->old_state = m_state ;
							this->m_state = new_state; 
							remain_time = 0; 
//					std::cout<<new_state.velocity<<' '<<new_state.position<<std::endl ;
							break; 
						}
					}
					m_color -= m_color_decrease ;
					this->m_lifespan -= 1.0 ;
				}
//		value_type get_time_step() const {return this->time_step ;}
				state_type & get_state () {return this->m_state ;}
				state_type & get_oldstate() {return this->old_state ;}

				color_type & get_color () {return this->m_color ;}
				void set_object (std::vector<mesh_type *>  * v_mesh)
				{
					mp_boundary = v_mesh; 
				}
				bool is_live () const {return m_lifespan > value_type(0.0) ;} 
				void set_avoid_acc (const vector_type & acc){this->m_avoid_acc = acc ; }
				void set_center_acc (const vector_type & acc) {this->m_center_acc = acc ;}

				void set_velocity_acc (const vector_type & acc ){this->m_velocity_acc = acc ;}
				void set_center (const vector_type & center) {this->m_center = center ;}
			protected:
		
			private:
				state_type new_state ;
				state_type m_state; 
				state_type old_state ;
				value_type time_step ;
				value_type m_lifespan ;
				vector_type m_color_decrease ;

				//flocking variable. 
				vector_type m_avoid_acc ;
				vector_type m_center_acc ;
				vector_type m_velocity_acc ;

				vector_type m_pray_position ;

				color_type m_color ;
				static constexpr value_type time_max = 10.0; 
				std::vector<mesh_type *> *  mp_boundary; 
				std::vector<unsigned> v_face_id; 
				vector_type m_center ;

			}; 

		struct  boid : public object_base  <boid_tag>
		{
			typedef typename object_base::vector_type::value_type value_type ;
			typedef typename object_base::vector_type vector_type; 
			typedef typename object_base::vector_type point_type; 
		    boid () : object_base (), m_neighbor_count (1){}
			void set_neighbor_count (unsigned c ) {this->m_neighbor_count = c ;}
			unsigned get_neighbor_count () const { return this->m_neighbor_count ;}
			void set_fartest_neighbor_distance (value_type dis ) {this->m_fartest_neighbor_distance = dis ;}

			void set_neighbors (const std::vector<unsigned> & neighbor) 
			{
				m_neighbors = neighbor ;
			}
		protected :
			unsigned m_neighbor_count ;
			std::vector <unsigned > m_neighbors ;
			value_type m_fartest_neighbor_distance ; 
		};

		template <typename object_> 
			struct system_base 
			{
				typedef float value_type ;
				typedef object_ object_type; 
				typedef math::random_generator<value_type, math::uniform_tag, -1, 1> random_type ;
				typedef math::pbm::boid_state <math::vector <value_type, 3> > state_type ;

				void object_generator (object_type & obj) {}
				void update () 
				{
				}
			protected:
				system_base() {} ;
				~system_base() {} ;
//				std::vector <object_type> m_objects ;
			} ;

		template <typename object_type> struct system {} ;
		template <> struct system <boid> : public system_base <boid> 
		{
			typedef typename system_base::value_type value_type ;
			typedef typename system_base::object_type object_type ;
			typedef typename system_base::object_type::vector_type vector_type; 
			typedef vector<float, 3> color_type ;
			typedef typename system_base::random_type random_type ;
			typedef typename system_base::state_type state_type ;

			// We need kd tree here. 
			typedef math::kdtree <value_type , 3> tree_type ;
			typedef math::kdres <value_type, 3> res_type ;

			void object_generator (object_type & obj)
			{
				float x = m_generator.random() ;
				float y = m_generator.random() ;
				float z = m_generator.random() ; 
				m_initial_state.velocity = x + m_initial_state.direction.x
					, y + m_initial_state.direction.y , z + m_initial_state.direction.z; 
				color_type color ((x + 1.0 ) / 2.0, (y + 1.0) /2.0, (z+1.0)/2.0);  

				obj.initialize(m_initial_state, 0.01, m_generator.random() * 75, color);	
			} 

			
			
			void update () 
			{
				build_kd_tree() ;
				static unsigned frame = 0; 
				std::cout<<"Frame "<<frame++<<std::endl;
				vector_type center = compute_center(); 
				vector_type pray_pos ;
				update_pray_position(center, pray_pos); 
				std::vector<unsigned> ne_idx ;
				for (unsigned i = 0; i < m_objects.size(); ++i)
				{
					compute_neighbor (i, 0.5, 10, ne_idx);
					compute_neighbor_avoid_acc(i, ne_idx);
					compute_neighbor_velocity_acc(i, ne_idx);
					compute_center_acc (i, center );
					m_objects[i].set_pray_position(pray_pos);
					m_objects[i].set_center (center);
				}

				clear_kd_tree() ;
				for (unsigned i = 0; i < m_objects.size(); ++i)
					m_objects[i].update(); 
			}
			void update_pray_position (const vector_type & center, vector_type & pray_pos)
			{
				vector_type p1 (10, 0, -5);
				vector_type p2 (-10, 0, 5);
				
				static bool flag = false; 
				if (flag == true && distance2(center,  p1) < 1) 
				{
					flag = false; 
				}
				else if  (flag == false && distance2(center,  p2) < 1)
				{
					flag = true; 
				}
				if (flag == true) pray_pos = p1 ; else pray_pos = p2 ;
			}
			system  () 
			{
				m_generator.seed(5);
				const unsigned num_of_boid = 1000;
				m_objects.resize (num_of_boid);
				m_initial_state.velocity = vector_type (0, 0, 10); 
				m_initial_state.position = vector_type (-10, 0, -.5); 
				m_initial_state.direction = vector_type (0, 0, 2);
				for (unsigned i = 0; i < num_of_boid; ++i)
				{
					object_generator(m_objects[i]);
				}
			}
			unsigned get_object_num () const {return this->m_objects.size();}
			const object_type & get_object (unsigned idx ) const {return this->m_objects[idx];}
			object_type & get_object (unsigned idx ) {return this->m_objects[idx];}
			vector_type compute_center ()
			{
				vector_type center (0, 0, 0);
				for (unsigned i = 0; i < m_objects.size(); ++i)
				{
					center += m_objects[i].get_state().position ; 
				}
				center /= value_type(m_objects.size());
				return center ;
			}

			void compute_center_acc (unsigned query_idx , vector_type & center)
			{
				vector_type & pquery = m_objects[query_idx].get_state().position ;
				const value_type kc = .2f ;
			
				m_objects[query_idx].set_center_acc ((center - pquery ) * kc);
				
			}

			void compute_neighbor (unsigned query_idx, value_type range , unsigned k , std::vector<unsigned> & ne_idx)
			{
				ne_idx.resize(k); 
				find_range_n (query_idx, range, k , ne_idx);
				/*
				for (unsigned i = 0; i < ne_idx.size(); ++i)
				{
					std::cout<<ne_idx[i]<<' '; 
				}
				std::cout<<std::endl;				
				*/
			}
			void compute_neighbor_velocity_acc (unsigned query_idx, const std::vector<unsigned> & ne_idx)
			{
				vector_type & pquery = m_objects[query_idx].get_state().velocity ;
				vector_type velocity_acc (0.0, 0.0, 0.0);
				for (unsigned i = 0; i < ne_idx.size(); ++i)
				{
					const value_type kv = 2.0; 
					vector_type & pn = m_objects[ne_idx[i]].get_state().velocity ;
					value_type dis = distance2 (pn, pquery);
					velocity_acc += kv * (pn - pquery)  ;

				}
				m_objects[query_idx].set_velocity_acc(velocity_acc);				
			}
			void compute_neighbor_avoid_acc (unsigned query_idx,  const std::vector<unsigned> & ne_idx)
			{

				// avoidance
				vector_type & pquery = m_objects[query_idx].get_state().position ;
				vector_type avoid_acc (0.0, 0.0, 0.0);
				for (unsigned i = 0; i < ne_idx.size(); ++i)
				{
					const value_type ka = 3.0; 
					vector_type & pn = m_objects[ne_idx[i]].get_state().position ;
					value_type dis = distance2 (pn, pquery);
					if (std::abs(dis) < 1e-5) dis = 1e-5;
					avoid_acc += (pn - pquery) / dis  * (-ka) ;
					// Overlap, add a fartest acceleration. 
					if (avoid_acc.module2() < 1e-5 ) 
						avoid_acc = 100, 100, 100;
				}
				m_objects[query_idx].set_avoid_acc(avoid_acc);
//				std::cout<<query_idx<<" Avoid ACC "<<avoid_acc<<std::endl;

			}
			
		protected :
			void build_kd_tree () 
			{

				if(!(data = (unsigned *)malloc(get_object_num() * sizeof (unsigned)))) {
					std::cout<<"Fail to allocate data"<<std::endl;
					throw ("Fail to allocate data");
				}
				mp_tree = new tree_type ;
				for (unsigned i = 0; i < m_objects.size() ; ++i)
				{
					data[i] = i ;
					mp_tree->kd_insert(m_objects[i].get_state().position, &data[i]);
				}
			}
			void clear_kd_tree ()
			{
				free(data);

				delete mp_tree ;
			}
			
			void find_range_n (unsigned idx, value_type range, unsigned k, std::vector <unsigned> & ne_idx)
			{
				vector_type pt = m_objects[idx].get_state().position ;
				//mp_result = mp_tree->kd_nearest_range(  pt, range , 1);
				mp_result = mp_tree->kd_nearest_range_n(  pt, range, k + 1 , 1); // unordered. 

				if (NULL == mp_result) 
				{
					std::cerr<<"Fail to query."<<std::endl;
					throw ("Fail to query kdtree.");
				}
				vector_type pos ;
				unsigned count = 0; 
				while( !mp_result->kd_res_end() ) {
					unsigned * pch = (unsigned *)mp_result->kd_res_item(pos );
					value_type dist = distance (pt, pos); 
					//printf( "node at (%.3f, %.3f, %.3f) is %.3f away and has data=%u\n", 
					//		pos[0], pos[1], pos[2], dist, *pch );
					if (*pch != idx) ne_idx[count++] = *pch ;
					if (count >=k) break; 
					mp_result->kd_res_next();
				}
				ne_idx.resize(count); 

//				std::cout<<"trace end."<<std::endl;
				mp_result->kd_res_free(); 
				free(mp_result);
//				std::cout<<"free trace end."<<std::endl;
			}

			void find_range (unsigned idx, value_type range)
			{
				vector_type pt = m_objects[idx].get_state().position ;
				value_type radius = range; 
				mp_result = mp_tree->kd_nearest_range(  pt, radius , 1);
				vector_type pos ;
				while( !mp_result->kd_res_end() ) {
					unsigned * pch = (unsigned *)mp_result->kd_res_item(pos );
					value_type dist = distance (pt, pos); 
					//printf( "node at (%.3f, %.3f, %.3f) is %.3f away and has data=%u\n", 
					//		pos[0], pos[1], pos[2], dist, *pch );
					mp_result->kd_res_next();
				}
			}

		protected :
			state_type m_initial_state ;
			random_type m_generator ;
			std::vector<object_type> m_objects ;
			// KD-query
			tree_type * mp_tree ;
			res_type * mp_result ;
			unsigned * data  ;
			
		} ;
	}
}

#endif
